/**
 * @file RobotCtrlType.h
 * @author jozenlee (2250017028@qq.com)
 * @brief
 * @version 0.1
 * @date 2022-03-06
 *
 * @copyright Copyright (c) 2022
 *
 */

#ifndef _ROBOTCTRLTYPE_H_
#define _ROBOTCTRLTYPE_H_

#include <common/LegData.h>
#include <common/RobotCommand.h>

typedef enum JOINT {
    JOINT_RF1,
    JOINT_RF2,
    JOINT_RF3,

    JOINT_LF1,
    JOINT_LF2,
    JOINT_LF3,

    JOINT_RB1,
    JOINT_RB2,
    JOINT_RB3,

    JOINT_LB1,
    JOINT_LB2,
    JOINT_LB3,

    JOINTS_ALL
} JOINT;

typedef enum LEG {
    LEG_RF,
    LEG_LF,
    LEG_RB,
    LEG_LB,

    LEG_NUM
} LEG;

typedef enum QT_Cmd {
    QT_Joint,
    QT_Foot,
    QT_Gait

} QT_Cmd;

typedef enum Leg_Mode {
    Mode_Pos,
    Mode_Tau,
} Leg_Mode;

typedef enum AU_Type {
    AU_NULL,
    AU_FRONT,
    AU_LEFT,
    AU_RIGHT
} AU_Type;

/*!
 * Command to robot
 */
struct RobotCommand {
    float q_des_abad[4];
    float q_des_hip[4];
    float q_des_knee[4];

    float qd_des_abad[4];
    float qd_des_hip[4];
    float qd_des_knee[4];

    float kp_abad[4];
    float kp_hip[4];
    float kp_knee[4];

    float kd_abad[4];
    float kd_hip[4];
    float kd_knee[4];

    float tau_abad_ff[4];
    float tau_hip_ff[4];
    float tau_knee_ff[4];

    int32_t flags[4];   // 使能位

    // 控制模式，只对仿真环境有效 
    // 0 位控 1 力控
    int32_t mode_abad[4];
   int32_t mode_hip[4];
   int32_t mode_knee[4];
};

/*!
 * Data from spine board
 */
struct LegData {
    float q_abad[4];
    float q_hip[4];
    float q_knee[4];
    float qd_abad[4];
    float qd_hip[4];
    float qd_knee[4];
    int32_t flags[4];   
    bool available(void){
        if(flags[0] && flags[1] && flags[2] && flags[3]) return true;
        else return false;
    }
};

/**
 * @brief This package contain only two legs' data because the control command will be sent to two departed board on the robot
 * @note  head frame and tail frame are used to gurantee the integrity of data
 *
 */
#pragma pack(1)
typedef struct _UsartCmdType {
    uint8_t head;
    /* Data */
    uint16_t q_des_abad[2];
    uint16_t q_des_hip[2];
    uint16_t q_des_knee[2];

    uint16_t qd_des_abad[2];
    uint16_t qd_des_hip[2];
    uint16_t qd_des_knee[2];

    uint16_t kp_abad[2];
    uint16_t kp_hip[2];
    uint16_t kp_knee[2];

    uint16_t kd_abad[2];
    uint16_t kd_hip[2];
    uint16_t kd_knee[2];

    uint16_t tau_abad_ff[2];
    uint16_t tau_hip_ff[2];
    uint16_t tau_knee_ff[2];

    uint8_t flags[2];
    /* ------ */
    uint8_t tail;
} UsartCmdType;
#pragma pack()

/**
 * @brief This package contain only two legs' data because the control command will be sent to two departed board on the robot
 * @note  head frame and tail frame are used to gurantee the integrity of data
 *
 */
#pragma pack(1)
typedef struct _UsartDataType {
    uint8_t head;
    /* Data */
    uint16_t q_abad[2];
    uint16_t q_hip[2];
    uint16_t q_knee[2];

    uint16_t qd_abad[2];
    uint16_t qd_hip[2];
    uint16_t qd_knee[2];

    uint8_t flags[2];
    /* ------ */
    uint8_t tail;
} UsartDataType;
#pragma pack()

void convertToMsg(const LegData* , common::LegData* );
void convertToMsg(const RobotCommand* , common::RobotCommand* );
void convertFromMsg(const common::LegDataConstPtr,  LegData* );
void convertFromMsg(const common::RobotCommandConstPtr , RobotCommand* );

#endif